#include "Entity.h"
#include "ForceGeneratorSpring.h"
#include "Vector3.hpp"

#include "math.h"

using namespace std;

ForceGeneratorSpring::ForceGeneratorSpring(float constant, float distance) : ForceGenerator()
{	
	springConstant = constant;
	springDistance = distance;

	springRegister = new map<int, Entity*>();
}

ForceGeneratorSpring::~ForceGeneratorSpring()
{	}

//Calculate spring force if particle is registered
void ForceGeneratorSpring::Register(int id, Entity *source, Entity *other)
{
	if (ForceGenerator::Register(id, source))
	{
		if (springRegister->find(id) == springRegister->end())
		{
			springRegister->insert(pair<int, Entity*>(id, other));
		}
		else
		{
			entityRegister->erase(entityRegister->find(id));
		}
	}
}

void ForceGeneratorSpring::Update(int id, float time)
{
	if (entityRegister->find(id) != entityRegister->end())
	{
		Vector3* force = new Vector3(*(*entityRegister)[id]->Position());
		(*force) -= *((*springRegister)[id]->Position());

		float magnitude = force->Magnitute();
		magnitude = fabs(magnitude - springDistance);
		magnitude *= springConstant;

		force->Normalize();
		(*force) *= -magnitude;
		(*entityRegister)[id]->AddForce(*force);
	}
}